
using System;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using FarseerGames.FarseerXNAPhysics.Dynamics;
using FarseerGames.FarseerXNAPhysics.Mathematics;

namespace Traffic.Path
{  
    public class SimpleVehicle
    {                     

        private float _mass;
        public float Mass
        {
            get { return this._mass; }
            set { this._mass = value; }
        }

        private float _max_force;
        private float _max_velocity;
        
        private float _orientation;        
        public float Orientation
        {
            get { return this._orientation; }
            set { this._orientation = value % Calculator.PiX2; }
        }

        private Vector2 _position;
        public Vector2 Position
        {
            get { return _position; }
            set { this._position = value; }
        }

        private Vector2 _velocity;
        public Vector2 Velocity
        {
            get { return this._velocity; }
            set { this._velocity = value; }
        }

        private Vector2 _steeringDirection;
        public Vector2 SteeringDirection
        {
            get { return _steeringDirection; }
            set { _steeringDirection = value; }
        }

        public SimpleVehicle()
        {          
            Position = new Vector2();
            Velocity = new Vector2();
            _max_velocity = 200f;
            _max_force = 50f;
        }        
       
        public void Initialize()
        {                                            
            this.Position = new Vector2(100f, 100f);                                       
        }

        /// <summary>
        /// Truncate a vector to given maximum length
        /// </summary>
        /// <param name="orig"></param>
        /// <param name="max"></param>
        public void Truncate(ref Vector2 orig, float max_len)
        {            
            float orig_len = orig.Length();

            if (orig_len > max_len)
            {
                orig *= (max_len / orig_len);
            }
        }

        /// <summary>
        /// Update the position of the vehicle
        /// </summary>
        /// <param name="dt"></param>
        public void Update(float dt)
        {
                //steering_force = truncate (steering_direction, max_force)
                //acceleration = steering_force / mass
                //velocity = truncate (velocity + acceleration, max_speed)
                //position = position + velocity
           
            Vector2 steering_force;
            Vector2 acceleration;

            steering_force = SteeringDirection;
            Truncate(ref steering_force, _max_force);

            acceleration = steering_force / Mass;

            Velocity = Velocity + (acceleration * dt);
            Truncate(ref _velocity, _max_velocity);

            Position += (Velocity * dt);          
            
            // Update orientation if we're moving
            if (Velocity.LengthSquared() > 1f)
            {
                Vector2 o = Velocity;
                o.Normalize();
                Orientation = (float)Math.Acos(o.X);
            }

        }


        /// <summary>
        /// Steers the vehicle towards a given point
        /// </summary>
        /// <param name="goal"></param>
        public void SteerToward(Vector2 goal)
        {
            Vector2 desired_velocity;
            Vector2 steering;

            // desired_velocity = normalize (position - target) * max_speed
            // steering = desired_velocity - velocity

            desired_velocity = Position - goal;
            desired_velocity.Normalize();
            desired_velocity *= _max_velocity;
            SteeringDirection = desired_velocity - Velocity;

         }
    }
}



